Skip to content


ai  101  pytorch  classification  nvidia  cuda  install  tensorrt  yolo  ardupilot  None  ros2  dds  micro ros  xrce  sitl  plugin  SITL  debug  rangefinder  pymavlink  mavros  gazebo  distance sensor  system_time  timesync  cmake  gtest  ctest  cpp  c++  format  fmt  multithreading  spdlog  camera  coordinate system  orb  matching  opencv  build  transformation  computer vision  homography  optical flow  of  trackers  cv  cyclonedds  eprosima  fastdds  simulation  config  ignition  bridge  sdf  tips  ign-transport  sensors  lidar  aptly  apt  encryption  pgp  docker  git  bundle  github  hooks  pre-commit  lxd  container  lxc  x11  profile  vscode  marpit  presentation  marp  markdown  mermaid  video  ffmpeg  gstreamer  cheat-sheet  sdp  v4l2loopback  gi  snippets  cheat Sheet  python  asyncio  future  click  cli  numpy  project  template  black  isort  docs  project document  docstrings  flake8  linter  git-hook  mypy  unittest  pytest  pylint  mock  iterator  generator  logging  tuple  namedtuple  typing  annotation  pyzmq  zmq  msgpack  action  namespace  remap  control2  ros2_control  gdb  qos  tag  plugins  msg  node  zero-copy  shm  tutorial  algorithm  calibration  diff  pid  dev  colcon  colcon_cd  rpi  arm  qemu  settings  behavior  plot  visualization  debugging  diagnostic  diagnostics  tutorials  gst  math  apm  rat_runtime_monitor  web  rosbridge  vue  binding  discovery  gazebo-classic  launch  spawn  cook  gps  imu  ray  gazebo_ros_ray_sensor  ultrsonic  range  ultrasonic  gazebo classic  wrench  effort  odom  ign  gz  xacro  ros_ign  diff_drive  odometry  joint_state  argument  OpaqueFunction  DeclareLaunchArgument  LaunchConfiguration  tmux  nav  slam  test  rclpy  executor  MultiThreadedExecutor  SingleThreadedExecutor  param  dynamic-reconfigure  service  client  setup.py  package.xml  parameter  parameters  custom  msgs  executers  pub  sub  rqt  rviz  rviz2  pose  marker  tf2  deb  package  setup  local_setup  rosdep  package manager  project settings  vcstool  cross-compiler  nano  texture  tmuxp  rootfs  embedded  zah  linux  rm  ubuntu  ip  ss  network  netstat  snap  deploy  ssh  systemd  mkdocs  extensions  socat  networking  serial  udp  tc  mtu  select  px4  robotics  kalman_filter  kalman  filter  control  todo  vscode-ext  json  yaml  schema  yocto  poky  world  gazebo_ros2_control  position_controller  effort_controller  velocity_controller  urdf  gazebo_ros_force  gazebo_ros_joint_state_publisher  robot_state_publisher  joint_state_publisher  projects  vrx  buoyancy 

Create Action server with multithread executer


Table of Content

Demo#

action server with multithread
import rclpy
import threading
import time
from rclpy.executors import MultiThreadedExecutor
from rclpy.action import ActionServer
from rclpy.node import Node
from rclpy.action.server import ServerGoalHandle

from action_tutorial_interfaces.action import MyAction

TOPIC = "my_action_demo"
PERIOD = 1

class MyActionServer(Node):
    def __init__(self):
        super().__init__('my_action_server')
        self._action_server = ActionServer(
            self,
            MyAction,
            TOPIC,
            self.execute_callback)
        self.__timer = self.create_timer(PERIOD, self.__timer_handler)
        self.__timer
        self.get_logger().info('Start my_action_server version')

    def __timer_handler(self):
        self.get_logger().info("timer thread ->: {}".format(threading.current_thread().ident))


    def execute_callback(self, goal_handle: ServerGoalHandle):
        self.get_logger().info("action thread ->: {}".format(threading.current_thread().ident))
        feedback_msg = MyAction.Feedback()
        for i in range(goal_handle.request.count):
            time.sleep(1)
            self.get_logger().info('current: {}'.format(i))
            feedback_msg.current = i
            goal_handle.publish_feedback(feedback_msg)

        self.get_logger().info('Action ended')
        goal_handle.succeed()

        result = MyAction.Result()
        result.total = i
        return result


def main(args=None):
    rclpy.init(args=args)
    action_server = MyActionServer()
    exec = MultiThreadedExecutor()
    exec.add_node(action_server)
    exec.spin()
    rclpy.spin(action_server)


if __name__ == '__main__':
    main()

Run#

run action
ros2 action send_goal -f /my_action_demo action_tutorial_interfaces/action/MyAction "{count: 3}"
server output
[INFO] [1670248402.659192149] [my_action_server]: timer thread ->: 140488007521856
[INFO] [1670248403.657669396] [my_action_server]: timer thread ->: 140487998469696
[INFO] [1670248404.005261426] [my_action_server]: action thread ->: 140487998469696
[INFO] [1670248404.658963720] [my_action_server]: timer thread ->: 140488007521856
[INFO] [1670248405.006998918] [my_action_server]: current: 0
[INFO] [1670248405.658867664] [my_action_server]: timer thread ->: 140487990076992
[INFO] [1670248406.009163793] [my_action_server]: current: 1
[INFO] [1670248406.658945210] [my_action_server]: timer thread ->: 140488015914560
[INFO] [1670248407.011518224] [my_action_server]: current: 2
[INFO] [1670248407.012507114] [my_action_server]: Action ended
[INFO] [1670248407.658774598] [my_action_server]: timer thread ->: 140488015914560